#ifndef __POSESOLVER_
#define __POSESOLVER_

#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include <opencv2/rgbd/depth.hpp>
#include "SendData.h"
#include <memory.h>
#include <string.h>

using namespace rs2;

class PoseSolver;
using p_posesolver = std::shared_ptr<PoseSolver>;

class PoseSolver
{
public:
    PoseSolver(){};
    ~PoseSolver(){};

    SendData resolute(const cv::Mat &, const rs2_intrinsics &, const cv::Rect &, const float&);
    cv::Mat getColorMask();

private:
    // 计算平面法向量
    cv::Mat _PlaneNormal(cv::Mat &, cv::Mat &, cv::Mat &);
    // 单位化平面法向量
    cv::Vec3f _unitNormal(const cv::Mat &);
    // 欧拉角解算
    cv::Vec3f _normalToEuler(const cv::Vec3f &);
    // 平面掩膜色彩渲染
    void _colorMask(cv::Mat &, cv::Mat &);
    // 获取平面重心
    cv::Point _getPlaneCentroid(cv::Mat &);
    // 获取面积最大平面
    int _getMaxPlane(cv::Mat &);
    // 计算世界坐标系下的坐标
    float* _goalToObject(const cv::Rect &, const float &);

private:
    cv::Mat _depth;
    cv::Mat _depRoi;

    rs2_intrinsics _intrinsics;
    
    cv::Mat _Mat3D;
    cv::Mat _planeNormal;
    cv::Mat _mask;
    cv::rgbd::RgbdPlane _rgbdPlane;
};

#endif
